/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <sys/time.h>
#include <string.h>


/********** Local Includes ****************************************************/

#include "livesource.h"
#include "objectdetector.h"
#include "approxDecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "simpledrivecontroller.h"
#include "rovio.h"
#include "astarheuristics.h"

/********** Macros and Constants **********************************************/

#define DEBUG true
#define ROVIO_HOST "192.168.10.18"
#define TIME 500

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Functions ***************************************************/

Point robot(231, 241), fruit(662, 241);
/**
 * A static helper function that returns the time with millisecond precision.
 */

int main(int, char **) {

    RobotOrientation current;
    Command command;
    cv::Point goal;
    SimpleDriveController *controller = new SimpleDriveController();
    string str;
    srand(time(NULL));

    int i =0;
    while (i<1)
    {
         i++;
         current.point.x = rand()%100;
         current.point.y = rand()%300;
         goal.x = rand()%100;
         goal.y = rand()%300;
         current.angle = rand()%360;

         current.point.x = 420;
         current.point.y = 188;
         goal.x = 400;
         goal.y = 140;
         current.angle = 360 - 87.7094;

         //current.angle = 0;
         command = controller->nextOmni(current,goal);
         switch(command.direction){
             case 0:
             str = "DIR_STOP";
             break;
             case 1:
             str = "DIR_FORWARD";
             break;
             case 2:
             str = "DIR_BACKWARD";
             break;
             case 3:
             str = "DIR_LEFT";
             break;
             case 4:
             str = "DIR_RIGHT";
             break;
             case 5:
             str = "DIR_ROT_LEFT";
             break;
             case 6:
             str = "DIR_ROT_RIGHT";
             break;
             case 7:
             str = "DIR_DIAG_FOR_LEFT";
             break;
             case 8:
             str = "DIR_DIAG_FOR_RIGHT";
             break;
             case 9:
             str = "DIR_DIAG_BACK_LEFT";
             break;
             case 10:
             str = "DIR_DIAG_BACK_RIGHT";
             break;
             case 11:
             str = "HEAD_UP";
             break;
             case 12:
             str = "HEAD_DOWN";
             break;
             case 13:
             str = "HEAD_MIDDLE";
             break;
             default:
             str = "invalid direction";
         }



         cout << "Difference: (" << goal.x - current.point.x <<", "<< -goal.y + current.point.y<<")\tAngle: " << (atan2(goal.y - current.point.y,goal.x - current.point.x)/M_PI*180)-current.angle <<"\tDirection: " << str << endl;

    }

}


